#include "CLineSegment2d.h"
#include "CMatrix2d.h"
#include "CVector2d.h"






CVector2d& CVector2d::ProjOnto( const CVector2d& v )
{
	ASSERT( FALSE ); // Not yet implemented

	return *this;
}


CVector2d& CVector2d::LerpTo( const CVector2d& v, fixed u )
{
	m_i = m_i + CMathFixed::Mul( u, v.m_i - m_i );
	m_j = m_j + CMathFixed::Mul( u, v.m_j - m_j );

	return *this;
}


CVector2d& CVector2d::Rotate( fixed degrees )
{
	if( degrees )
	{
		fixed sin = CMathFixed::Sin( degrees );
		fixed cos = CMathFixed::Cos( degrees );

		fixed i = m_i;

		m_i = CMathFixed::Mul( cos, i ) + CMathFixed::Mul( -sin, m_j );
		m_j = CMathFixed::Mul( sin, i ) + CMathFixed::Mul( cos, m_j );
	}

	return *this;
}


fixed CVector2d::Angle() const
{
	CVector2d n( *this );

	n.Normalize();

	fixed ang = CMathFixed::ACos( n.m_i );

	if( m_j < 0 && ang )
		ang = CMathFixed::Degrees_360 - ang;

	return ang;
}


fixed CVector2d::Angle( CVector2d& v ) const
{
	fixed mLen = Length() * v.Length();

	if( mLen )
		return CMathFixed::ACos( ( *this * v ) / mLen );
	else
		return 0;
}


fixed CVector2d::AngleTo( CVector2d& v ) const
{
	fixed	a1 = Angle();
	fixed	a2 = v.Angle();
	fixed	aDiff = a2 - a1;

	if( aDiff <= -CMathFixed::Degrees_180 )
		aDiff += CMathFixed::Degrees_360;
	else 
	if( aDiff >= CMathFixed::Degrees_180 )
		aDiff -= CMathFixed::Degrees_360;

	return aDiff;
}


fixed CVector2d::ShortestDistanceToLineSegment( const CVector2d& segV0, const CVector2d& segV1,
												CVector2d& pntOfIntersection ) const
{
	fixed dwv, dvv;
	CVector2d v = segV1 - segV0; 
	CVector2d w = *this - segV0; 

	dwv = w * v;
	dvv = v * v;

	if( dwv <= 0 )
	{
		w = segV0 - *this;
		pntOfIntersection = segV0;
	}
	else
	if( dvv <= dwv )
	{
		w = segV1 - *this;
		pntOfIntersection = segV1;
	}
	else
	{
		w = *this;

		pntOfIntersection = v;
		pntOfIntersection *= CMathFixed::Div( dwv, dvv );
		pntOfIntersection += segV0;

		w -= pntOfIntersection;
	}

	return w.Length();
}


fixed CVector2d::ShortestDistanceToLineSegment( class CLineSegment2d& segment, 
											    CVector2d& pntOfIntersection ) const
{
	return ShortestDistanceToLineSegment( segment.m_v0, segment.m_v1, pntOfIntersection );
}


/*  
 *  Source:
 *  Area of Triangles and Polygons (2D & 3D)
 *  Dan Sunday
 *  http://geometryalgorithms.com/Archive/algorithm_0101/
 */

// tests if a point is Left|On|Right of an infinite line.
//    Input:  three points pP0, pP1, and pPTest
//    Return: >0 for pPTest left of the line through pP0 and pP1
//            =0 for pPTest on the line
//            <0 for pPTest right of the line
fixed CVector2d::IsOnLineSegment( const CVector2d& segV0, const CVector2d& segV1 ) const
{
	fixed v1, v2;

	v1 = CMathFixed::Mul( segV1.m_i - segV0.m_i, m_j - segV0.m_j );
	v2 = CMathFixed::Mul( m_i - segV0.m_i, segV1.m_j - segV0.m_j );

    return v1 - v2;
}


fixed CVector2d::IsOnLineSegment( class CLineSegment2d& segment ) const
{
    return IsOnLineSegment( segment.m_v0, segment.m_v1 );
}


/*
 *  Source: Fast Winding Number Inclusion of a Point in a Polygon 
 *  Dan Sunday
 *  http://geometryalgorithms.com/Archive/algorithm_0103/algorithm_0103.htm
 *
 *  *The Winding Number*
 *
 *  On the other hand, the winding number accurately determines if a point is inside 
 *  a nonsimple closed polygon.  It does this by computing how many times the 
 *  polygon winds around the point.  A point is outside only when the polygon 
 *  doesn't wind around the point at all which is when the winding number *wn*= 0.  
 *  More generally, one can define the winding number *wn* around a point for any 
 *  closed continuous curve in the 2D plane.  Let C(/u/)=(/x/(/u/),/y/(/u/)), for 
 *  0<=/u/<=1 with C(0)=C(1), be a continuous 2D curve, and let P be a point not on 
 *  C(/u/).  Then, define the vector C_P (/u/)=C(/u/)-P  from P to C(/u/), and its 
 *  unit direction vector is W(/u/)=C_P (/u/)/|C_P (/u/)| which gives a continuous 
 *  mapping from the curve C to the unit circle S1 , and so can be represented in 
 *  coordinates as W(/u/)=(cos q(/u/), sin q(/u/)) where q(/u/) is a positive 
 *  counterclockwise angle.  The winding number * wn* is then equal to the integer 
 *  number of times W(/u/) wraps around S1 .  This corresponds to a homotopy class 
 *  of S1 , and can be computed by the integral:
 *  
 *  When the curve C is a polygon with vertices V_0 ,V_1 ,...,V_/n/ =V_0 , this 
 *  integral reduces to the sum of the (signed) angles that each edge V_/i/ V_/i/+1 
 *  subtends with the point P.  So, if q_/i/ =angle(PV_/i/ ,PV_/i/+1 ), we have:
 *  
 *  This formula is clearly not very efficient since it uses an expensive arccos() 
 *  trig function.  But, a simple observation lets us replace this formula by a more 
 *  efficient one.  Pick any point Q on S1 . Then, as the curve W(/u/) wraps around 
 *  S1 , it passes Q a certain number of times.  If we count (+1) when it passes Q 
 *  counterclockwise, and (-1) when it passes clockwise, then the accumulated sum is 
 *  exactly the total number of times that W wraps around S1 , and is equal to the 
 *  winding number *wn*.  Further, if we take an infinite ray R starting at P and 
 *  extending in the direction of the vector Q, then intersections where R crosses 
 *  the curve C correspond to the points where W passes Q.  To do the math, we have 
 *  to distinguish between positive and negative crossings where C crosses R from 
 *  right-to-left or left-to-right.  This can be determined by the sign of the dot 
 *  product between a normal vector to C and the direction vector Q [Foley et al, 
 *  1996, p-965], and when the curve C is a polygon, one just needs to make this 
 *  determination once for each edge.  For a horizontal ray R from P, testing 
 *  whether an edge's endpoints are above and below the ray suffices.  If the edge 
 *  crosses the positive ray from below to above, the crossing is positive (+1); but 
 *  if it crosses from above to below, the crossing is negative (-1). One then 
 *  simply adds all crossing values to get *wn*.
 *  
 *  Additionally, one can avoid computing the actual edge-ray intersection point by 
 *  using the isLeft() <#isLeft()> attribute; however, it needs to be applied 
 *  differently for ascending and descending edges.  If an upward edge crosses the 
 *  ray to the right of P, then P is on the left side of the edge since the triangle 
 *  V_/i/ V_/i/+1 P is oriented counterclockwise.  On the other hand, a downward 
 *  edge crossing the positive ray would have P on the right side since the triangle 
 *  V_/i/ V_/i/+1 P would then be oriented clockwise.
 */

// winding number test for point-in-polygon
// returns the winding number (wn)
// the point is outside -- wn = 0
// the point is inside -- wn != 0
fixed CVector2d::IsInPoly( const CVector2d* vertex, int vertexCount ) const
{
	int32 wn = 0;

    for( int32 i = 0; i < vertexCount; i++ )   
	{
		int32 idx = ( i + 1 < vertexCount ) ? i + 1 : 0;

        if( ( vertex + i )->m_j <= m_j )                 
		{
			// an upward crossing
			// P left of or on edge
            if( ( vertex + idx )->m_j > m_j )            
			{
                if( IsOnLineSegment( *( vertex + i ), *( vertex + idx ) ) >= 0 ) 
                    wn++;                               
			}
        }
        else                        
		{
			// a downward crossing
			// P right of or on edge
            if( ( vertex + idx )->m_j <= m_j )           
			{
                if( IsOnLineSegment( *( vertex + i ), *( vertex + idx ) ) <= 0 ) 
                    wn--;                               
			}
        }
    }

	return wn;
}


CVector2d& CVector2d::operator*=( const class CMatrix2d& a )
{
	fixed i = m_i;

	m_i = CMathFixed::Mul( a[0], i ) + CMathFixed::Mul( a[2], m_j );
	m_j = CMathFixed::Mul( a[1], i ) + CMathFixed::Mul( a[3], m_j );

	return *this;
}


CVector2d operator*( const class CMatrix2d& a, const CVector2d& x )
{
	return CVector2d( CMathFixed::Mul( a[0], x.m_i ) + CMathFixed::Mul( a[2], x.m_j ),
					  CMathFixed::Mul( a[1], x.m_i ) + CMathFixed::Mul( a[3], x.m_j ) );
}


